#ifndef __DWA_H__
#define __DWA_H__

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/point_cloud2_iterator.h>
#include <nav_msgs/Path.h>
#include "../local_planner_base.h"

//轨迹
struct Trajectory{
    vector<Pose2D> waypoints;     //bsee_frame坐标系下的预测轨迹
    Velocity2D vel_2d;            //对应速度数据
    float traj_score;             //轨迹得分
    float heading_score;          //航向角得分
    float speed_score;            //速度得分
    float goal_score;             //终点距离得分
    float obstacle_score;         //障碍物得分
};

// https://wiki.ros.org/dwa_local_planner


// 【ROS-Navigation】—— DWA路径规划算法解析
// https://blog.csdn.net/sinat_52032317/article/details/128816594
// 机器人局部避障的动态窗口法(dynamic window approach)
// https://blog.csdn.net/heyijia0327/article/details/44983551
// 自动驾驶路径规划DWA算法原理解析
// https://blog.csdn.net/gophae/article/details/101393017
// 局部路径规划 DWA 算法完全解析（理论推导+代码实现，包你看懂！）
// https://blog.csdn.net/Solititude/article/details/131863621
// 路径规划算法（一）：DWA动态窗口算法
// https://blog.csdn.net/qq_38768959/article/details/122860891
// 路径规划算法C++实现（三）--DWA
// https://blog.csdn.net/weixin_44504228/article/details/115698377
// ROS-Navigation包中DWA算法研究一（DWA算法介绍）
// https://blog.csdn.net/wwyklnh/article/details/103868425
// Dynamic Window Approach_机器人局部避障的动态窗口法
// https://blog.csdn.net/subiluo/article/details/81912732
// 路径规划-DWA算法（C++实现）
// https://blog.csdn.net/u011573853/article/details/131362981
// 【路径规划】局部路径规划算法——DWA算法（动态窗口法）|（含python实现 | c++实现）
// https://blog.csdn.net/weixin_42301220/article/details/127769819
// 【局部路径规划算法】—— DWA动态窗口法（c++实现））
// https://bigdavid.blog.csdn.net/article/details/137360042

class DWAPlanner : public LocalPlannerBase{
    public:
        DWAPlanner(tf2_ros::Buffer* tf_buffer);

        /********************
         *  虚函数实现
         ********************/
        // 更新缓存数据
        void UpdateCache();
        // 计算速度指令
        bool ComputeTwist(Velocity2D& cmd_vel, LidarScan2D lidar_scan_2d, 
            Pose2D robot_pose, Pose2D goal_pose);
        

        // 计算轨迹
        void GenerateTrajectory(Trajectory& traj, float predicted_time, float dt);
        // 发布点云数据
        void PublishPointCloud2(vector<Trajectory> predicted_trajs);
        // 发布最佳轨迹
        void PublishBestTraj(Trajectory best_traj);
        // 计算刹车距离
        float StopDistance(Velocity2D vel_2d, float acc);
        // 计算航向角度评分  轨迹末端点角度和末端点与目标点连线的角度差
        float HeadingScore(Pose2D traj_point, Pose2D goal);
        // 速度评分
        float VelocityScore(Velocity2D vel_2d);
        // 计算障碍物评分  计算轨迹末端点距离最近障碍物距离的评价函数 碰撞检测
        bool ObstaclesScore(Pose2D traj_point, Velocity2D vel_2d, vector<Pose2D>& obstacles, 
            float inflation_radius, float& min_distance);
        // 碰撞检测
        bool ObstaclesCollision(vector<Pose2D> waypoints, vector<Pose2D>& obstacles, 
            float inflation_radius);
        // 查找最佳轨迹
        bool FindBestTrajectory(Trajectory& best_traj, LidarScan2D& lidar_scan_2d, 
            Pose2D robot_pose_2d, Pose2D goal_pose_2d);

    private:
        ros::NodeHandle __nh;
        ros::Publisher __dwa_predicted_trajs_pub;
        ros::Publisher __dwa_best_traj_pub;

    private:
        Velocity2D __max_vel = Velocity2D(1.5, 1.5, 2.0);           //最大速度限制
        Velocity2D __min_vel = Velocity2D(-1.5, -1.5, -2.0);        //最小速度限制
        Velocity2D __vel_samples = Velocity2D(0.1, 0.1, 0.15);       //速度采样分辨率
        vector<Velocity2D> __vel_space;         //速度空间
        vector<Trajectory> __predicted_trajs;   //速度空间对应的轨迹缓存
        float __predicted_time;
        bool __rotate2goal = false;
        
};


#endif